#ifndef COMPASS_H
#define COMPASS_H

#include "board.h"
#include "ahrs.h"

extern float    mag[3];

typedef struct
{
  //unsigned short   compass_raw[3];    // raw data from compass
  float   compass_zero[3];   // calibration data
  float   compass_scale[3];  // calibration data
} Compass;

extern Compass compass;

/*unsigned short IMU_ADC_MAGX_ZERO;// = 33000;
  unsigned short IMU_ADC_MAGY_ZERO;// = 33000;
  unsigned short IMU_ADC_MAGZ_ZERO;// = 33000;

  float IMU_ADC_MAGX_SCALE;// = 1.0f;
  float IMU_ADC_MAGY_SCALE;// = 1.0f;
  float IMU_ADC_MAGZ_SCALE;// = 1.0f;*/

//extern unsigned short   compass_raw[3];   // raw data from compass
//extern unsigned short   compass_zero[3];  // calibration data
//extern float            compass_scale[3]; // calibration data

//extern unsigned short   compass_calibration_zero[3];  // calibration data
//extern float            compass_calibration_scale[3]; // calibration data

// Initialize compass zero and scale
//extern inline void compass_init(void);
extern void compass_init(void);

// Calculate calibrated compass data (x axis)
extern void compass_update_x(unsigned short compass_raw_x);
// Calculate calibrated compass data (y axis)
extern void compass_update_y(unsigned short compass_raw_y);
// Calculate calibrated compass data (z axis)
extern void compass_update_z(unsigned short compass_raw_z);

extern void compass_calibrate(Compass* compas_calibration);

#endif


